#include <DeformingPolygon.h>
#include <map>
#include <szMiscOperations.h>
#include <szMexUtility.h>
#include <IntersectionConvexPolygons.h>
#include <mex.h> //just for printf

/*
A unit vector from o to x.
*/
CParticleF 
NormalizedDirection(CParticleF& x, CParticleF& o)
{
	float dx = x.m_X - o.m_X;
	float dy = x.m_Y - o.m_Y;
	float len = sqrt(dx*dx + dy*dy);
	if(len > 1.0e-5)
	{
		return CParticleF(dx/len, dy/len);
	}
	else
	{
		return CParticleF(0.0f, 0.0f);
	}
}

/*
return a unit vector that bisects the angle formed by three points: a-o-b.
*/
CParticleF
bisector(CParticleF& o, CParticleF& a, CParticleF& b)
{
	CParticleF x = NormalizedDirection(a, o);
	CParticleF y = NormalizedDirection(b, o);
	CParticleF z((x.m_X+y.m_X)/2, (x.m_Y+y.m_Y)/2);
	float vx = z.m_X;
	float vy = z.m_Y;
	float len0 = sqrt(vx*vx + vy*vy);
	if(len0 <= 1.0e-10) //this is a colinear point. 
	{
		float ang = GetVisualDirection(b.m_X, b.m_Y, a.m_X, a.m_Y) + PI/2.0;
		vx = cos(ang);
		vy = sin(ang);
	}
	else
	{
		vx = vx / len0;
		vy = vy / len0;
	}
	return CParticleF(vx, vy);
}

float
intersectPlaneAndLine(CParticleF op, CParticleF& z, CParticleF& ol, CParticleF u)
{
	//move the origin to ol.
	op.m_X -= ol.m_X;
	op.m_Y -= ol.m_Y;
	op.m_Z -= ol.m_Z;
	float t = (z.m_X * op.m_X + z.m_Y * op.m_Y + z.m_Z * op.m_Z) / (z.m_X * u.m_X + z.m_Y * u.m_Y + z.m_Z * u.m_Z);
	return t;
}

bool
coLinear(CParticleF& a, CParticleF& b, CParticleF& c, float precision = 1.0e-3)
{
	float d = Distance2Line(a, c, b);
	return d < precision;
}

bool
coLinear(CParticleF& a, CParticleF& b, CParticleF& c, CParticleF& d, float precision = 1.0e-3)
{
	float d1 = Distance2Line(a, c, b);
	float d2 = Distance2Line(a, c, d);
	return d1 < precision && d2 < precision;
}

CParticleF
crossProduct(CParticleF& a, CParticleF& b)
{
	return CParticleF(a.m_Y*b.m_Z-a.m_Z*b.m_Y, a.m_Z*b.m_X-a.m_X*b.m_Z, a.m_X*b.m_Y-a.m_Y*b.m_X);
}

bool
calculateVelocity(CParticleF& a, CParticleF& o, CParticleF& b, float& vx, float& vy)
{
	CParticleF bs = bisector(o, a, b);
	float ang = GetVisualAngle2(a.m_X, a.m_Y, b.m_X, b.m_Y, o.m_X, o.m_Y);
	float cs = cos((PI - Abs(ang)) / 2.0);
	float len = 1.0f / cs;
	bs.m_X *= len;
	bs.m_Y *= len;
	if(ang<0)
	{
		bs.m_X = -bs.m_X;
		bs.m_Y = -bs.m_Y;
	}
	vx = bs.m_X;
	vy = bs.m_Y;
	return true;
}

bool 
MovingParticle::setVelocity()
{
	if(next && prev)
	{
		if(id == 38)
			id += 0;
		calculateVelocity(prev->p, p, next->p, v[0], v[1]);
		return true;
	}
	else
	{
		return false;
	}
}


DeformingPolygon::DeformingPolygon(vector<CParticleF>& pnts)
{
	ParticleFactory* factory = ParticleFactory::getInstance();
	float eps = 1.0e-5;
	for(int i=0; i<pnts.size(); ++i)
	{
		MovingParticle* p = factory->makeParticle(pnts[i]);
		particles.push_back(p);
	}
	for(int i=0; i<particles.size(); ++i)
	{
		//particles[i]->p0 = particles[i]->p;
		int j = (i+1) % particles.size();
		particles[i]->next = particles[j];
		int k = (i-1+particles.size()) % particles.size();
		particles[i]->prev = particles[k];
		particles[i]->setVelocity();
	}
}

float
DeformingPolygon::nextEdgeEvent()
{
	float t = std::numeric_limits<float>::infinity();
	for(int i=0; i<particles.size(); ++i)
	{
		CParticleF p2 = particles[i]->move(1);
		CParticleF q2 = particles[i]->next->move(1);
		pair<float,float> param = _IntersectConvexPolygon::intersect(particles[i]->p, p2, particles[i]->next->p, q2);
		float t0 = Max(param.first, param.second);
		if(param.first>0 && param.second >0 && t0 < t)
		{
			t = t0;
		}
	}
	return t;
}

float
DeformingPolygon::nextSplitEvent()
{
	float t = std::numeric_limits<float>::infinity();
	for(int i=0; i<particles.size(); ++i)
	{
		CParticleF bs = bisector(particles[i]->p, particles[i]->prev->p, particles[i]->next->p);
		if(bs.m_X * particles[i]->v[0] + bs.m_Y * particles[i]->v[1] < 0) //concave
		{
			CParticleF ol = particles[i]->p;
			CParticleF ul(particles[i]->v[0], particles[i]->v[1], 1.0);
			for(int j=0; j<particles.size(); ++j)
			{
				if(particles[i]==particles[j] || particles[i]->next==particles[j] || particles[i]->prev==particles[j]) continue;
				CParticleF op = particles[j]->p;
				CParticleF oq = particles[j]->next->p;
				CParticleF ov = NormalizedDirection(op, oq);
				CParticleF up0(ov.m_Y, -ov.m_X, 1.0f);
				CParticleF up = crossProduct(ov, up0);
				if(up.m_X*particles[j]->v[0] + up.m_Y*particles[j]->v[1] < 0)
				{
					up.m_X = -up.m_X;
					up.m_Y = -up.m_Y;
				}
				float t0 = intersectPlaneAndLine(op, up, ol, ul);
				if(t0 >= 0 && t0<t) 
				{
					//check if the intersection point is within the sweep of the line segment.
					CParticleF loc(ol.m_X+t0*ul.m_X, ol.m_Y+t0*ul.m_Y);
					CParticleF op2 = particles[j]->move(t0); 
					CParticleF oq2 = particles[j]->next->move(t0); 
					if(Distance2LineSegment(op2,  oq2, loc)<1.0f) //1.0f is arbitrary. I thought it is small enough but maybe too big.
					{
						t = t0;
					}
				}
			}
		}
	}
	return t;
}

DeformingPolygon
DeformingPolygon::split(int begin, int end)
{
	ParticleFactory* factory = ParticleFactory::getInstance();
	DeformingPolygon poly(*this);
	vector<MovingParticle*> copy = particles;

	vector<MovingParticle*> particles1;
	particles1.push_back(factory->makeParticle(copy[begin]->p));
	for(int i=(begin+1) % copy.size(); i!=(end+1) % copy.size(); i=(i+1) % copy.size())
	{
		particles1.push_back(copy[i]);
	}
	for(int i=0; i<particles1.size(); ++i)
	{
		int i0 = i==0 ? particles1.size()-1: i-1;
		int i2 = i==particles1.size()-1 ? 0: i+1;
		particles1[i]->prev = particles1[i0];
		particles1[i]->next = particles1[i2];
		particles1[i]->setVelocity();
	}

	vector<MovingParticle*> particles2;
	particles2.push_back(factory->makeParticle(copy[begin]->p));
	for(int i=(end+1) % copy.size(); i!=begin; i=(i+1) % copy.size())
	{
		particles2.push_back(copy[i]);
	}
	for(int i=0; i<particles2.size(); ++i)
	{
		int i0 = i==0 ? particles2.size()-1: i-1;
		int i2 = i==particles2.size()-1 ? 0: i+1;
		particles2[i]->prev = particles2[i0];
		particles2[i]->next = particles2[i2];
		particles2[i]->setVelocity();
	}

	if(particles1.size() < particles2.size())
	{
		this->particles = particles2;
		poly.particles = particles1;
	}
	else
	{
		this->particles = particles1;
		poly.particles = particles2;
	}
	return poly;
}

bool
DeformingPolygon::sanityCheck(int& error_code, int& error_loc)
{
	for(int i=0; i<particles.size(); ++i)
	{
		int i0 = i==0 ? particles.size()-1: i-1;
		int i2 = i==particles.size()-1 ? 0: i+1;
		if(particles[i]->prev != particles[i0])
		{
			error_code = 1;
			error_loc = i;
			return false;
		}
		else if(particles[i]->next != particles[i2])
		{
			error_code = 2;
			error_loc = i;
			return false;
		}
		for(int j=i+1; j<particles.size(); j++)
		{
			if(particles[i] == particles[j])
			{
				error_code = 3;
				error_loc = i;
				return false;
			}
		}
	}
	return true;
}

vector<DeformingPolygon>
fixPolygon(DeformingPolygon& polygon, float eps)
{
	ParticleFactory* factory = ParticleFactory::getInstance();
	//find any duplicates (as the result of edge events)
	while(polygon.particles.size()>1)
	{
		bool bChanged = false;
		for(int i=0; i<polygon.particles.size(); ++i)
		{
			MovingParticle* pi = polygon.particles[i];
			MovingParticle* pj = polygon.particles[i]->next;
			if(pj == NULL) break; //there must be only one point.

			if(Distance(pi->p, pj->p) < eps)
			{
				//create a new particle and insert it between the colliding particles in the array
				MovingParticle* p = factory->makeParticle(pi->p);
				if(pi->prev && pi->prev != pj) 
				{
					p->prev = pi->prev;
					p->prev->next = p;
				}
				if(pi->next->next && pi->next->next != pi)
				{
					p->next = pi->next->next;
					p->next->prev = p;
				}
				p->setVelocity();

				polygon.particles.insert(polygon.particles.begin()+i, p);
				polygon.particles.erase(find(polygon.particles.begin(), polygon.particles.end(), pj));
				polygon.particles.erase(find(polygon.particles.begin(), polygon.particles.end(), pi));
				bChanged = true;
				break;
			}
		}
		if(bChanged == false) break;
	}
	//find colinear redundant points
	while(polygon.particles.size()>2)
	{
		bool bChanged = false;
		for(int i=0; i<polygon.particles.size(); ++i)
		{
			if(coLinear(polygon.particles[i]->prev->p, polygon.particles[i]->p, polygon.particles[i]->next->p))
			{
				polygon.particles[i]->prev->next = polygon.particles[i]->next;
				polygon.particles[i]->next->prev = polygon.particles[i]->prev;
				polygon.particles[i]->prev->setVelocity();
				polygon.particles[i]->next->setVelocity();
				polygon.particles.erase(polygon.particles.begin()+i);
				bChanged = true;
				break;
			}
		}
		if(bChanged == false) break;
	}
	//find intersection (as the result of split events) or overlap (as the result of edge events)
	vector<DeformingPolygon> splitted;
	while(polygon.particles.size()>3)
	{
		bool bChanged = false;
		for(int i=0; i<polygon.particles.size(); ++i)
		{
			int loc = -1;
			for(int j=0; j<polygon.particles.size(); ++j)
			{
				int j0 = (j-1+polygon.particles.size()) % polygon.particles.size();
				int j2 = (j+1) % polygon.particles.size();
				if(i==j || i==j2) continue;
				float d = Distance2LineSegment(polygon.particles[j]->p, polygon.particles[j2]->p, polygon.particles[i]->p);
				if(d < eps)
				{
					loc = j;
					break;
				}
			}
			if(loc >= 0)
			{
				DeformingPolygon poly2 = polygon.split(i, loc);
				if(poly2.particles.size() > 2)
				{
					splitted.push_back(poly2);
				}
				bChanged = true;
				break;
			}
		}
		if(bChanged == false) break;
	}
	return splitted;
}

void
_trace(Vertex<CParticleF>* u, vector<CParticleF>& points)
{
	points.push_back(u->key);
	u->color = Black;
	for(int i=0; i<u->aList.size(); ++i)
	{
		Vertex<CParticleF>* v = u->aList[i]->v;
		if(v->color == White)
		{
			_trace(v, points);
			points.push_back(u->key);
		}
	}
}

vector<CParticleF>
traceTree(vector<Vertex<CParticleF>*>& tree)
{
	for(int i=0; i<tree.size(); ++i)
	{
		tree[i]->Reset();
		//rotate neighbors by orientation
		vector<pair<float,Edge<CParticleF>*>> pairs;
		for(int j=0; j<tree[i]->aList.size(); ++j)
		{
			Vertex<CParticleF>* v = tree[i]->aList[j]->v;
			float ang = GetVisualDirection(v->key.m_X, v->key.m_Y, tree[i]->key.m_X, tree[i]->key.m_Y);
			pairs.push_back(pair<float,Edge<CParticleF>*>(ang, tree[i]->aList[j]));
		}
		sort(pairs.begin(), pairs.end());
		tree[i]->aList.clear();
		for(int j=0; j<pairs.size(); ++j)
		{
			tree[i]->aList.insert(tree[i]->aList.begin(), pairs[j].second);
		}
	}

	vector<CParticleF> points;
	_trace(tree[0], points);
	return points;
}

/*vector<CParticleF>
traceTree(vector<Vertex<CParticleF>*>& tree)
{
	map<Vertex<CParticleF>*,int> imap;
	for(int i=0; i<tree.size(); ++i)
	{
		imap[tree[i]] = i;
	}

	vector<CParticleF> points;
	vector<int> numVisits(tree.size(), 0);
	Vertex<CParticleF>* start = tree[0];
	Vertex<CParticleF>* p = start;
	Vertex<CParticleF>* prev =  NULL;;
	while(true)
	{
		printf("trace %d\n", imap[p]+1);
		points.push_back(p->key);
		numVisits[imap[p]] ++;
		Vertex<CParticleF>* next =  NULL;
		float minang = 3*PI; //bigger than any
		float eps = 1.0e-5; 
		//go through adjacent vertices that still need to be visited and find the one that is firt in the CW order.
		for(int i=0; i<p->aList.size(); ++i)
		{
			Vertex<CParticleF>* v = p->aList[i]->v;
			if(numVisits[imap[v]] < v->aList.size())
			{
				if(prev==NULL) next = v; //very first time
				else
				{
					float ang = GetVisualAngle2(prev->key.m_X, prev->key.m_Y, v->key.m_X, v->key.m_Y, p->key.m_X, p->key.m_Y);
					if(ang < eps) ang += 2*PI;
					if(ang < minang)
					{
						minang = ang;
						next = v;
					}
				}
			}
		}
		if(next == NULL) break; //Done.
		prev = p;
		p = next;
	}
	return points;
}*/

vector<CParticleF>
offsetPolygon(vector<CParticleF>& points, float margin, float eps)
{
	vector<CParticleF> poly;
	for(int i=0; i<points.size(); ++i)
	{
		int i0 = (i-1+points.size()) % points.size();
		int i2 = (i+1) % points.size();
		CParticleF o = points[i];
		CParticleF a = points[i0];
		CParticleF b = points[i2];
		if(Distance(a, o) < eps || Distance(b, o) < eps) continue;
		if(Distance(a, b) > eps)
		{
			float vx, vy;
			calculateVelocity(a, o, b, vx, vy);
			CParticleF q(o.m_X + margin * vx, o.m_Y + margin * vy);
			poly.push_back(q);
			printf("o[%3.3f,%3.3f] -> [%3.3f,%3.3f]\n", o.m_X, o.m_Y, q.m_X, q.m_Y);
		}
		else //this is a leaf node of the tree.
		{
			float ang = GetVisualDirection(o.m_X, o.m_Y, a.m_X,  a.m_Y);
			float angles[] = {ang+PI/4.0, ang-PI/4.0};
			for(int j=0; j<2; ++j)
			{
				CParticleF q(o.m_X+sqrt(2.0) * margin*cos(angles[j]), o.m_Y+sqrt(2.0)*margin*sin(angles[j]));
				printf("\to[%3.3f,%3.3f] -> [%3.3f,%3.3f]\n", 
					o.m_X, o.m_Y, q.m_X, q.m_Y);
				poly.push_back(q);
			}
			/*float angles[] = {ang+PI/2.0, ang-PI/2.0}; //IS THIS THE RIGHT ORDER FOR CLOCK-WISE SEQUENCE???
			for(int j=0; j<2; ++j)
			{
				CParticleF b2(o.m_X + cos(angles[j]), o.m_Y + sin(angles[j]));
				float vx, vy;
				if(ClockWise(b2.m_X, b2.m_Y, o.m_X, o.m_Y, a.m_X, a.m_Y))
				{
					calculateVelocity(a, o, b2, vx, vy);
				}
				else
				{
					calculateVelocity(b2, o, a, vx, vy);
				}
				CParticleF q(o.m_X + margin * vx, o.m_Y + margin * vy);
				poly.push_back(q);
				printf("\to[%3.3f,%3.3f] -> [%3.3f,%3.3f], b2=[%3.3f,%3.3f], v=[%3.3f,%3.3f]\n", 
					o.m_X, o.m_Y, q.m_X, q.m_Y, b2.m_X, b2.m_Y, vx, vy);
			}*/
		}
	}
	return poly;
}


